#pragma once
#include <string>
#include <memory>
#include <list>
#include <thread>
#include <atomic>
#include <ompl/base/SpaceInformation.h>
#include <ompl/base/spaces/SE2StateSpace.h>
#include <ompl/base/spaces/SE3StateSpace.h>
#include <ompl/base/spaces/SO2StateSpace.h>
#include <ompl/geometric/planners/rrt/RRTConnect.h>
#include <ompl/geometric/SimpleSetup.h>
#include "LocationInfo.h"
namespace ob = ompl::base;
namespace og = ompl::geometric;
namespace Planner
{
class OMPLPlanner
{
    public:
        typedef enum SPACE_TYPE
        {
            SPACE_TYPE_NONE=0,
            SPACE_TYPE_SE2,
            SPACE_TYPE_SE3,
            SPACE_TYPE_SO2,
            SPACE_TYPE_SO3
        }space_type_t;
        OMPLPlanner(int vechicle_id,
                    const LocationInfo& start_point,
                    const LocationInfo& target_point,
                    const std::map<int,LocationInfo>& obstacle_map,
                    space_type_t space_type=SPACE_TYPE_SE2,
                    float safe_distance=3.0);
        ~OMPLPlanner(){}
        OMPLPlanner(const OMPLPlanner&)=delete;
        OMPLPlanner& operator=(const OMPLPlanner&)=delete;
        bool isRunComplete(){return _this_planner_finished;}

        bool isSateValid(const ob::State *state);
        std::string pathJson(const std::vector<ob::State *> & path_vector);
        static void setOMPLBounds(double low_bound,double hight_bound);
    private:
        void OMPLPlannerThread();
        ob::StateSpacePtr _ompl_space_ptr;
        og::SimpleSetupPtr _ompl_setup_ptr;
        std::shared_ptr<std::thread> _ompl_planner_thread;
        std::atomic_bool _this_planner_finished;
        std::map<int,LocationInfo> _obstacle_map;
        float _safe_distance;
        int _vechicle_id;
        //commom property
        static ob::RealVectorBounds _ompl_bounds;
        //thread manage
        static std::list<std::shared_ptr<OMPLPlanner>> _ompl_planner_list;
        static std::shared_ptr<std::thread> _planner_manage_thread;
        static void addToManage(const std::shared_ptr<OMPLPlanner>& planner);
        static void OMPLPlannerThreadManage();
};
};